#include "navmesh.h"
extern "C" {
#include "c_navmesh.h"
}

struct CNavMesh : NavMesh {
  CNavMesh() {}
  ~CNavMesh() {}
};

CNavMesh *NewCNavMesh() {
  auto p = new CNavMesh();
  return p;
}

void DeleteMCNavMesh(CNavMesh *p) { delete p; }

bool Load(CNavMesh *p, const char *path) { return p->LoadSoloNavMesh(path); }

bool FindPath(CNavMesh *p, float spos[3], float epos[3], float *result,
              int *size) {
  p->FindPath(spos, epos);
  int index = 0;
  for (auto it : p->m_pathIndex) {
    float x = p->m_straightPath[(it)*3];
    float z = p->m_straightPath[(it)*3 + 1];
    float y = p->m_straightPath[(it)*3 + 2];
    result[index * 3] = x;
    result[index * 3 + 1] = z;
    result[index * 3 + 2] = y;
    index++;
  }
  *size = index;

  // memcpy(result, p->m_straightPath, p->m_nstraightPath * 3 * sizeof(float));
  // *size = p->m_nstraightPath;
  // for (int i = 0; i < p->m_nstraightPath; i++)
  // {
  // 	std::cout << p->m_straightPath[i * 3] << " " << p->m_straightPath[i * 3
  // + 2] << std::endl;
  // }

  return true;
}

bool RandomFindPath(CNavMesh *p, float *result, int *size) {
  float spos[3];
  float epos[3];

  dtPolyRef start_ref;
  dtPolyRef end_ref;

  dtStatus status =
      p->m_navQuery->findRandomPoint(&p->m_filter, frand, &start_ref, spos);
  if (dtStatusSucceed(status)) {
    status =
        p->m_navQuery->findRandomPoint(&p->m_filter, frand, &end_ref, epos);
    if (dtStatusSucceed(status)) {
      // std::cout << "spos:" << spos[0] << " " << spos[1] << " " << spos[2] <<
      // std::endl; std::cout << "epos:" << epos[0] << " " << epos[1] << " " <<
      // epos[2] << std::endl;
      p->FindPath(spos, epos);
      // for (int i = 0; i < p->m_nstraightPath * 3 && i < MAX_POLYS * 3; i++)
      // {
      // 	result[i] = p->m_straightPath[i];
      // }
      // int index = 0;
      // for (auto it : p->m_pathIndex)
      // {
      // 	float x = p->m_straightPath[(it)*3];
      // 	float z = p->m_straightPath[(it)*3 + 1];
      // 	float y = p->m_straightPath[(it)*3 + 2];
      // 	result[index * 3] = x;
      // 	result[index * 3 + 1] = z;
      // 	result[index * 3 + 2] = y;
      // 	index++;
      // }
      // *size = index;

      memcpy(result, p->m_straightPath, p->m_nstraightPath * 3 * sizeof(float));
      *size = p->m_nstraightPath;
      return true;
    }
  }
  return false;
}

bool FindRandomPoint(CNavMesh *p, float *start_pos, float *end_pos) {
  float spos[3];
  float epos[3];

  dtPolyRef start_ref;
  dtPolyRef end_ref;

  dtStatus status =
      p->m_navQuery->findRandomPoint(&p->m_filter, frand, &start_ref, spos);
  if (dtStatusSucceed(status)) {
    status =
        p->m_navQuery->findRandomPoint(&p->m_filter, frand, &end_ref, epos);
    if (dtStatusSucceed(status)) {
      memcpy(start_pos, spos, 3 * sizeof(float));
      memcpy(end_pos, epos, 3 * sizeof(float));
      return true;
    }
  }
  return false;
}